#include "slam.hpp"

#include <cmath>
#include <iostream>
#include <queue>

using namespace unitree::robot;

Slam::Slam()
{
    /* 初始化话题 */
    pubQtCommand->InitChannel();
    pubQtNode->InitChannel();
    pubQtEdge->InitChannel();

    /* 订阅话题 */
    subQtNotice->InitChannel(std::bind(&Slam::qtNoticeHandler, this, std::placeholders::_1), 10);
    subOdommetry->InitChannel(std::bind(&Slam::odometryHandler, this, std::placeholders::_1), 1);

    /* 初始化机器狗姿态 */
    relocation();
    std::cout << "Relocation command sent." << std::endl;

    nav_open();
    std::cout << "Navigation command sent." << std::endl;

    post_init();
    std::cout << "Post init command sent." << std::endl;

    sleep(1);
}

Slam::~Slam()
{
    pos_thread_stop();
}

void Slam::qtNoticeHandler(const void* message)
{
    int index_, begin_, end_, feedback_, arrive_;
    const std_msgs::msg::dds_::String_* seq = (const std_msgs::msg::dds_::String_*)message;
    std::string str_, notice_;

    begin_ = seq->data().find("index:", 0); // 指令识别码
    end_ = seq->data().find(";", begin_);
    str_ = seq->data().substr(begin_ + 6, end_ - begin_ - 6);
    index_ = atoi(str_.c_str());

    begin_ = seq->data().find("notice:", 0); // 提示消息
    end_ = seq->data().find(";", begin_);
    notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7);

    if (index_ <= 10000) { // 命令执行反馈
        begin_ = seq->data().find("feedback:", 0);
        end_ = seq->data().find(";", begin_);
        str_ = seq->data().substr(begin_ + 9, end_ - begin_ - 9);
        feedback_ = atoi(str_.c_str());
        if (feedback_ == 0 || feedback_ == -1)
            std::cout << "\033[1;31m" << "Command execution failed with index = " << index_ << "." << "\033[0m";
    } else if (index_ == 10001) { // 导航反馈
        begin_ = seq->data().find("arrive:", 0);
        end_ = seq->data().find(";", begin_);
        str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7);
        arrive_ = atoi(str_.c_str());
        std::cout << " I arrived node " << arrive_ << ". ";
    }
    std::cout << notice_ << std::endl;
}

void Slam::odometryHandler(const void* message)
{
    odom = (const nav_msgs::msg::dds_::Odometry_*)message;
}

void Slam::relocation()
{
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 6; // 6为开启重定位
    send_msg.attribute_() = 1; // 雷达型号1为xt16雷达，2为mid360雷达，请根据实际使用的雷达型号进行赋值
    pubQtCommand->Write(send_msg);
}

void Slam::post_init()
{
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 7; // 7为定位初始化指令
    send_msg.quaternion_x_() = 0; // 初始旋转姿态四元数
    send_msg.quaternion_y_() = 0;
    send_msg.quaternion_z_() = 0;
    send_msg.quaternion_w_() = 1;
    send_msg.translation_x_() = 0; // 初始位置XYZ坐标
    send_msg.translation_y_() = 0;
    send_msg.translation_z_() = 0;
    pubQtCommand->Write(send_msg);
}

void Slam::nav_open()
{
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 8; // 8为开启导航节点命令
    pubQtCommand->Write(send_msg);
}

void Slam::nav_start()
{
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 10; // 10 循环默认点导航命令
    pubQtCommand->Write(send_msg);
}

void Slam::nav_pause()
{
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 13; // 13为暂停导航命令
    pubQtCommand->Write(send_msg);
}

void Slam::nav_recover()
{
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 14; // 14为恢复导航命令
    pubQtCommand->Write(send_msg);
}

void Slam::pos_get(float& x, float& y, float& yaw)
{
    float siny_cosp;
    float cosy_cosp;

    while (odom == NULL) {
        usleep(40 * 1000);
    }

    float ow = odom->pose().pose().orientation().w();
    float ox = odom->pose().pose().orientation().x();
    float oy = odom->pose().pose().orientation().y();
    float oz = odom->pose().pose().orientation().z();

    x = odom->pose().pose().position().x();
    y = odom->pose().pose().position().y();

    siny_cosp = 2 * (ow * oz + ox * oy);
    cosy_cosp = 1 - 2 * (oy * oy + oz * oz);
    yaw = std::atan2(siny_cosp, cosy_cosp);
}

void Slam::pos_thread()
{
    bool valid = true;
    float x = 1.0;
    float y = 1.0;
    float yaw = 1.0;

    /* 等待 SLAM 服务的 ODOM 对象返回 */
    while (odom == NULL) {
        usleep(40 * 1000);
    }

    while (running) {
        pos_get(x, y, yaw);

        if (std::fabs(x) < 0.0001 || std::fabs(y) < 0.0001) {
            valid = false;
        } else if (std::fabs(x) > 100 || std::fabs(y) > 100) {
            valid = false;
        } else {
            valid = true;
        }

        PosInfo::set(valid, x, y, yaw);

        msleep(100);
    }
}

void Slam::pos_thread_start()
{
    if (!handle.joinable()) {
        running = true;
        handle = std::thread(&Slam::pos_thread, this);
    }
}

void Slam::pos_thread_stop()
{
    if (handle.joinable()) {
        running = false;
        handle.join();
    }
}

void Slam::msleep(int ms)
{
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}
